
#include "pid.h"
#include <string.h>

/* ---------------------------下面是PID的外部算法接口--------------------------- */
/**
 * @brief 初始化PID,设置参数和启用的优化环节,将其他数据置零
 * @param pid: PID实例
 * @param config: PID初始化设置
 */
void PIDInit(PIDInstance *pid, PID_Init_Config_s *config)
{
    // config的数据和pid的部分数据是连续且相同的的,所以可以直接用memcpy
    memset(pid, 0, sizeof(PIDInstance));
    // 利用其内存连续的结构体的特性, 将剩余内存设置为0
    memcpy(pid, config, sizeof(PID_Init_Config_s));
}

/**
 * @brief PID计算, 并反回PID输出
 * @param pid: PID实例指针
 * @param measure: 反馈值
 * @param set: 设定值
 */
float PIDCalculate(PIDInstance *pid, float set, float measure)
{
    // 保存上次的测量值和误差,计算当前error
    pid->Measure = measure;
    pid->Set = set;
    pid->Err = pid->Set - pid->Measure;

        // 基本的pid计算,使用位置式
        pid->Pout = pid->Kp * pid->Err;
        pid->ITerm = pid->Ki * pid->Err ;
        pid->Dout = pid->Kd * (pid->Err - pid->Last_Err);

        pid->Iout += pid->ITerm;                         // 累加积分
        pid->Output = pid->Pout + pid->Iout + pid->Dout; // 计算输出

    // 保存当前数据,用于下次计算
    pid->Last_Measure = pid->Measure;
    pid->Last_Output = pid->Output;
    pid->Last_Dout = pid->Dout;
    pid->Last_Err = pid->Err;
    pid->Last_ITerm = pid->ITerm;

    return pid->Output;
}

/**
 * @brief 清除PID, 将除初始化的其他数据置零
 * @param pid: PID实例
 */
void PIDClear(PIDInstance *pid)
{
    pid->Measure = pid->Last_Measure = pid->Err = pid->Last_Err = pid->Last_ITerm = 0.0f;
    pid->Pout = pid->Iout = pid->Dout = pid->ITerm = 0.0f;
    pid->Output = pid->Last_Output = pid->Last_Dout = pid->Set = pid->dt = 0.0f;
    pid->DWT_CNT = 0;
    pid->ERRORHandler.ERRORCount = 0;
    pid->ERRORHandler.ERRORType = PID_ERROR_NONE;
    pid->first_flag = 0;
}


// 循环限幅函数
float loop_float_constrain(float Input, float minValue, float maxValue)
{
	if (maxValue < minValue)
	{
		return Input;
	}

	if (Input > maxValue)
	{
		float len = maxValue - minValue;
		while (Input > maxValue)
		{
			Input -= len;
		}
	}
	else if (Input < minValue)
	{
		float len = maxValue - minValue;
		while (Input < minValue)
		{
			Input += len;
		}
	}
	return Input;
}

// 弧度格式化为-PI~PI
float rad_format(float Ang)
{
	return loop_float_constrain(Ang, -PI, PI);
}

